package edu.ncsu.csc.camera;

import java.io.*;
import java.lang.StringBuffer;
import com.sun.spot.util.Utils;

/**
 * @author Richard Pospesel
 * 
 * The CMUCam2 class provides methods to interract with a CMUCam2 camera
 * via input and output streams.  A subset of the CMUCam2 functions are 
 * implemented
 *
 * If using a custom stream (IE, as a wrapper around the UART methods found
 * in the SUNSpot for Serial communication), the streams only need to implement
 * in.read() and write(byte b)
 *
 * The various methods that receive packets work under the assumption that poll mode is enabled
 */

public class CMUCam2
{
	/** This stream sends commands to the camera */
	private InputStream in;
	/** This stream receives responses from the camera */
	private OutputStream out;	
	
	private boolean hi_res_mode = false;
	public int mins[];
	public int maxs[];
	
	/**
	 * Basic constructor, accepts an input stream and an output stream
	 * @param in inputstream to send commands to camera
	 * @param out outputstream to receive responses 
	 */
	
	public CMUCam2(InputStream in, OutputStream out)
	{
		this.in = in;
		this.out = out;
	}
	
	/**
	 * Some commands send packets repeatedly over the serial connection (TC for example).
	 * This command will make the CMUCam2 stop sending these packets.
	 * We can't just send an empty string via sendCommand because ACK is not returned when
	 * canceling a packet stream
	 * @return boolean always true
	 */
		
	public boolean sendBreak() throws IOException
	{
		System.gc();
		out.write((byte)'\r');
		//for(byte response = (byte)in.read(); response != -1; response = (byte)(in.read()));
		while(in.read() != -1);
		return true;		
	}
	
	/**
	 * This function sends a text command to the camera
	 * returns true if ACK\r is returned
	 * returns false if NCK\r is returned
	 * throws exception if neither is returned
	 * @param com an ASCII command to send to the camera
	 * @return boolean true on success, false on error
	 */
	
	public boolean sendCommand(String com) throws IOException
	{
//		System.out.println(": " + com);
		// kind of hacky; we listen on in until we're not getting anything anymore (ie, -1 is returned)
		// however, this could also return -1 if there are other problems!
		sendBreak();
		// send the command to camera
		int length = com.length();
		for(int k = 0; k < length; k++)
			out.write((byte)com.charAt(k));
		out.write(13);	// carriage return tells camera command finished
		
		// now we get the response
		byte[] response = new byte[4];
		for(int k = 0; k < 4; k++)
			response[k] = (byte)in.read();
		if(response[1] == 'C' && response[2] == 'K' && response[3] == 13)
		{
			if(response[0] == 'A')
				return true;
			else if(response[0] == 'N')
			{
				return false;
			}
		}
		System.out.print("err \"" + (0xff & response[0]) + ", " + (0xff & response[1]) + ", "+ (0xff & response[2]) + ", "+ (0xff & response[3])  + "\"\n");
		System.out.print("err \"" + (char)response[0] + (char)response[1] + (char)response[2] + (char)response[3]  + "\"\n");
		throw new IOException("Failed to receive ACK or NCK from CMUCam2");
	}
	
	/**
	 * This method returns a byte vector that contains a response from the camera
	 * It stops reading when it finds a carriage return (I need to check each command and see if this is OK)
	 * or if it reads a -1 (usually means nothing being sent from camera)
	 * @param expected_size this is the expected size (in bytes) of the response, allocates memory to read accordingly
	 * @return ByteVector response from the camera
	 */	
	
	public ByteVector readLine(int expected_size) throws IOException
	{
		ByteVector bv = new ByteVector(expected_size);
		int b;
		for(b = in.read(); b == -1; b = in.read());
		bv.add((byte)b);
		for(b = in.read(); b != 13 && b!= -1; bv.add((byte)b), b = in.read());
		
		return bv;		
	}
	
		/**
	 * This method returns a byte vector that contains a response from the camera
	 * It stops reading when it finds a carriage return (I need to check each command and see if this is OK)
	 * or if it reads a -1 (usually means nothing being sent from camera)
	 * @param bv this is a byte vector that has been preallocated
	 * @return ByteVector response from the camera
	 */	
	
	public ByteVector readLine(ByteVector bv) throws IOException
	{
		int b;
		for(b = in.read(); b == -1; b = in.read());
		bv.add((byte)b);
		for(b = in.read(); b != 13 && b!= -1; bv.add((byte)b), b = in.read());
		
		return bv;		
	}

	/*
		Here we start implementing commands
		commands that don't return a packet will return true or false depending on success
		commands that return a packet will return a packet on success and null on failure
	*/
	
	/**
	 * GV - Get Version
  	 * returns the version of the CMUcam2 we're using
	 * There seems to be a bug with GV on the camera end
	 * It does not return '\r' ':' like the documentation says each command should.
	 * It only returns ':' and then starts returning -1 since the camera isn't saying anything.
	 * Because of this, I simply write a string until I get to a ':' character
	 * @return String version of the camera we're using
	 */
	
	
	public String GV() throws IOException
	{
		if(sendCommand("GV"))
		{
			ByteVector bv = readLine(17);
			StringBuffer buffer = new StringBuffer();
			int bytes = bv.length();
			for(int k = 0; k < bytes; k++)
			{
				if(bv.get(k) == ':')
					break;
				buffer.append((char)bv.get(k));
			}
			return buffer.toString();
		}
		return null;
	}
	
	/**
	 * BM - Buffer Mode
	 * This method is used to determine how camera frames are stored.
	 * BM 1 - means only a single frame is in the buffer, frames are not continually sent
	 * 	With Poll Mode enabled, you can call multiple functions which will all work on the same frame
	 * 	One must call RF to load subsequent frames when in this mode.
	 * 	Also, tracking methods will only return a single T packet instead of a stream of them
	 * BM 0 - this is the default mode, new frames are continually dumped into the buffer
	 * @param active true to enable, false to disable
	 * @return boolean true on success, false on failure
	 */
	
	public boolean BM(boolean active) throws IOException
	{
		if(active)
			return sendCommand("BM 1");
		return sendCommand("BM 0");
	}
	
	/**
	 * CP - Camera Power
	 * This method toggles the camera modules power.  Can be used in cases where camera isn't always used
	 * 	CP 1 - turns the camera on
	 * 	CP 0 - turns the camera off
	 * @param on true to turn on, false to turn off
	 * @return boolean true on success, false on failure
	 */
	
	public boolean CP(boolean on) throws IOException
	{
		if(on)
			return sendCommand("CP 1");
		return sendCommand("CP 0");
	}
	
	/**
	 * CR - Camera Register
	 * This method allows you to change various variables stored in the camera (brightness, contrast, etc)
	 * Full explanations of the various registers can be found in the "Omnivision CMOS camera" documentation
	 *  CR register value
	 * @param register the register nuber we wish to change
	 * @param value the value to change the register to
	 * @return boolean true on success, false on failure
	 */
	 
	 public boolean CR(int register, int value) throws IOException
	 {
		if(register >=0 && register <= 255 && value >= 0 && value <= 255)
			return sendCommand("CR " + register + " " + value);
		return false;
	 }
	 
	 /**
	  * GH - Get Histogram
	  * This method returns an H Packet (which is a histogram) for the given channel
	  * There are either 28, 14, or 7 buckets depending on the set precision
	  * Precision can b eset with the HC command
	  *  GH 0 gets the histogram for red
	  *  GH 1 gets the histogram for green
	  *  GH 2 gets the histogram for blue
	  * @param channel the channel we want to get a histogram of
	  * @return HPacket a histogram 
	  */
	 
	public HPacket GH(int channel) throws IOException
	{
		if(channel >= 0 && channel <= 2 && sendCommand("GH " + channel))
		{
			// average_number_of_digits * maximum_number_of_bins + maximum_number_of_bins + H
			// 2 * 28 + 28 + 1 == 85
			ByteVector bv = readLine(85);
			StringBuffer buffer = new StringBuffer(bv.length());
			int bytes = bv.length();
			for(int k = 0; k < bytes; k++)
			{
				if(bv.get(k) == ':')
					break;
				buffer.append((char)bv.get(k));
			}
			
			return HPacket.build(buffer.toString());
		}
		return null;
	}
	
	/**
	 * GM - Get Mean
	 * This method returns an S Packet which contains the means for all the colours and the
	 * standard deviation of each colour
	 * @return SPacket returns the found S Packet
	 */
	 
	public SPacket GM() throws IOException
	{
		if(sendCommand("GM"))
		{
			ByteVector bv = readLine(25);
			StringBuffer buffer = new StringBuffer(bv.length());
			int bytes = bv.length();
			for(int k = 0; k < bytes; k++)
			{
				if(bv.get(k) == ':')
					break;
				buffer.append((char)bv.get(k));
			}
			return SPacket.build(buffer.toString());
		}
		return null;
	}
	
	/**
	 * Histogram Configure
	 * This method tells the camera how many bins to use when
	 * creating a histogram
	 * @param bins the number of histogram bins
	 * @return boolean returns true on success, false on failure
	 */
	 
	public boolean HC(int bins) throws IOException
	{
		return sendCommand("HC " + bins);
	}
	
	/**
	 * HiRes Mode
	 * This method enable or disables hi resolution mode
	 *  HR 1 - turns it on (176 x 255)
	 *  HR 0 - turns it off (88 x 143)
	 * @param state is it on or off?
	 * @return boolean return true on success, false on failure
	 */
	 
	public boolean HR(boolean state) throws IOException
	{
		if(state)
		{
			hi_res_mode = true;
			return sendCommand("HR 1");
		}
		hi_res_mode = false;
		return sendCommand("HR 0");
	}
	 
	 /**
	  * Poll Mode
	  * This method enables or disables poll mode
	  * When poll mode is disabled, a stream of packets is returned
	  * after receiving a command
	  * When it is on, you only receive one packet
	  * PM 1 - turns poll mode on
	  * PM 0 - turns poll mode off
	  * @param on poll mode is on or off
	  * @return boolean return true on success, false on failure
	  */
	  
	public boolean PM(boolean on) throws IOException
	{
		if(on)
			return sendCommand("PM 1");
		return sendCommand("PM 0");
	}
	
	/**
	 * Read Frame
	 * This reads a new frame into the buffer
	 * This is to be used when Buffer Mode is enabled
	 * @return boolean return true on success, false on failure
	 */
	
	public boolean RF() throws IOException
	{
		return sendCommand("RF");
	}
	
	/**
	 * Reset
	 * This command resets the vision board
	 * Doing this will set all registers to their default values
	 * @return boolean return true on success, false on failure
	 */
	 
	public boolean RS() throws IOException
	{
		hi_res_mode = false;
		return sendCommand("RS");
	}

	private int count_frame_bytes(int width, int height)
	{
		return (((width * 3) + 1) * height) + 4;
	}

	
	public FPacket SF() throws IOException
	{
		System.gc();
		byte[] buffer = null;
		if(hi_res_mode)
			buffer = new byte[count_frame_bytes(176,255)];
		else
			buffer = new byte[count_frame_bytes(88,143)];

		if(sendCommand("SF"))
		{
			in.read(buffer);
			ByteVector bv = new ByteVector(buffer);
			return FPacket.build(bv);
		}
		return null;
	}
	
	/**
	 * Virtual Window
	 * This command sets a sub frame from the camera buffer to do commands on
	 * @param x1 x coordinate of left edge of virtual window
	 * @param y1 y coordinate of top edge of virtual window
	 * @param x2 x coordinate of right edge of virtual window
	 * @param y2 y coordinate of bottom edge of virtual window
	 * @return boolean return true on success, false on failure
	 */
	
	public boolean VW(int x1, int y1, int x2, int y2) throws IOException
	{
		return sendCommand("VW " + x1 + " " + y1 + " " + x2 + " " + y2);
	}

	/**
	 * Virtual Window
	 * This command sets viewing window back to default window size
	 * @return boolean return true on success, false on failure
	 *
	 * NOTE: CMUCam2 documentation claims that VW with no arguments resets
	 * However, in reality the CMUCam2 emulation does not support VW with no arguments
	 * By digging through the CMUCam3 source, I've found that if you provide min and max arguments
	 * for x1 y1 x2 y2 it will default to the camera's resolution
	 */
	
	public boolean VW() throws IOException
	{
		return sendCommand("VW 0 0 255 255");
	}
	
	/**
	 * Track Color
	 * This method returns a TPacket for a given colour range
	 * This method works assuming Poll Mode is enabled
	 */
	 
	 public TPacket TC(int rmin, int rmax, int gmin, int gmax, int bmin, int bmax) throws IOException
	 {
		if(sendCommand("TC " + rmin + " " + rmax + " " + gmin + " " + gmax + " " + bmin + " " + bmax))
		{
			ByteVector bv = readLine(35);
			StringBuffer buffer = new StringBuffer(bv.length());
			int bytes = bv.length();
			for(int k = 0; k < bytes; k++)
			{
				if(bv.get(k) == ':')
					break;
				buffer.append((char)bv.get(k));
			}
			return TPacket.build(buffer.toString());
		}
		return null;
	 }
	
	/**
	 * Track Color
	 * This method tracks the color for the parameters given already
	 */
	 
	 public TPacket TC() throws IOException
	 {
		if(sendCommand("TC"))
		{
			ByteVector bv = readLine(35);
			StringBuffer buffer = new StringBuffer(bv.length());
			int bytes = bv.length();
			for(int k = 0; k < bytes; k++)
			{
				if(bv.get(k) == ':')
					break;
				buffer.append((char)bv.get(k));
			}
			return TPacket.build(buffer.toString());
		}
		return null;
	 }
	
	
	/** New Commands! **/
	
	/**
	 * This method will find the mean values for the colors found on screen
	 * It will then set the min and max rgb colours in TC to a certain percent
	 * brighter and darker than the mean values.
	 * @param change this is the value to knock brightness up and down
	 * 0.2 would be a 20% increase and decrease in brightness
	 * @return boolean return true on success, false on failure
	 */
	 
	public boolean CalibrateTracker(float change) throws IOException
	{
		// first set camera to low res mode (later throw in check for the current 
		// mode camera is in so we can reset it to hi res if it was there already)
		HR(false);
		// camera low resolution is now 88 x 143
		int width = 88;
		int height = 143;
		VW(width/3, height/3, 2*width/3, 2*height/3);
		// test file to make sure we're getting what I think we're getting
		//SF().toFile("TEST");
		// set histogram bins
		PM(true);

		SPacket means = GM();
		System.out.println(means);
		// reset the viewing window
		VW();
		// convert RGB to Y'CbCr colour space, reduce and increase brightness 
		// and convert back to RGB for min and max RGB values
		
		// found here: http://en.wikipedia.org/wiki/YCbCr
		int Y = (int)(0.257 * means.means[0] + 0.504 * means.means[1] + 0.098 * means.means[2] + 16);
		int Cb = (int)(-0.148 * means.means[0] - 0.291 * means.means[1] + 0.439 * means.means[2] + 128);
		int Cr = (int)(0.439 * means.means[0] - 0.368 * means.means[1] - 0.071 * means.means[2] +128);
		
		//System.out.println("YCbCr " + Y + ", " + Cb + ", " + Cr);
		
		mins = new int[3];
		maxs = new int[3];
		
		
		mins[0] = (int)((298.082 * (Y * (1.0 - change)) + 408.583 * Cr) / 256.0 - 222.921);
		maxs[0] = (int)((298.082 * (Y * (1.0 + change)) + 408.583 * Cr) / 256.0 - 222.921);
		
		mins[1] = (int)((298.082 * (Y * (1.0 - change)) - 100.291 * Cb - 208.120 * Cr)/256.0 + 135.576);
		maxs[1] = (int)((298.082 * (Y * (1.0 + change)) - 100.291 * Cb - 208.120 * Cr)/256.0 + 135.576);
		
		mins[2] = (int)((298.082 * (Y * (1.0 - change)) + 516.412 * Cb)/256.0 - 276.836); 
		maxs[2] = (int)((298.082 * (Y * (1.0 + change)) + 516.412 * Cb)/256.0 - 276.836);
		
		for(int k = 0; k < 3; k++)
		{
			if(mins[k] < 0)
				mins[k] = 0;
			else if(mins[k] > 255)
				mins[k] = 255;
			if(maxs[k] < 0)
				maxs[k] = 0;
			else if(maxs[k] > 255)
				maxs[k] = 255;
		}
		
		if(TC(mins[0], maxs[0], mins[1], maxs[1], mins[2], maxs[2]) != null)
		{
			System.out.println(TC());
			return true;
		}
		return false;
	}
	
}
